#include "rotation.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>


float p_copy[4];
float p_transform[XF_DIMS][XF_DIMS];

float* copy(float *points){

   p_copy[0]=points[0];
   p_copy[1]=points[1];
   p_copy[2]=points[2];
   p_copy[3]=points[3];
   return p_copy;
}

void rotateWX(float*points, float rad){
    float * pt = copy(points);
    points[0]= pt[0]*cos(rad)-pt[1]*sin(rad);
    points[1]= pt[0]*sin(rad)+pt[1]*cos(rad);
}

void rotateXY(float*points, float rad){
    float * pt = copy(points);
    points[1]= pt[1]*cos(rad)-pt[2]*sin(rad);
    points[2]= pt[1]*sin(rad)+pt[2]*cos(rad);
}

void rotateYZ(float*points, float rad){
    float * pt = copy(points);
    points[2]= pt[2]*cos(rad)-pt[3]*sin(rad);
    points[3]= pt[2]*sin(rad)+pt[3]*cos(rad);
}

void rotateZW(float*points, float rad){
    float * pt = copy(points);
    points[3]= pt[3]*cos(rad)-pt[0]*sin(rad);
    points[0]= pt[3]*sin(rad)+pt[0]*cos(rad);
}

void rotateWY(float*points, float rad){
    float * pt = copy(points);
    points[0]= pt[0]*cos(rad)-pt[2]*sin(rad);
    points[2]= pt[0]*sin(rad)+pt[2]*cos(rad);
}

void rotateXZ(float*points, float rad){
    float * pt = copy(points);
    points[1]= pt[1]*cos(rad)-pt[3]*sin(rad);
    points[3]= pt[1]*sin(rad)+pt[3]*cos(rad);
}

static void matrix_mul(float vals[XF_DIMS][XF_DIMS]) {
	float tmp[XF_DIMS][XF_DIMS];
	int i,j,k;
	float f;
	for (i = 0; i < XF_DIMS; i++)
		for (j = 0; j < XF_DIMS; j++) {
			f = 0;
			for (k = 0; k < XF_DIMS; k++)
				f += p_transform[i][k] * vals[k][j];
			tmp[i][j] = f;
		}
	for (i = 0; i < XF_DIMS; i++)
		for (j = 0; j < XF_DIMS; j++)
			p_transform[i][j] = tmp[i][j];
}

void matrix_ident(float vals[XF_DIMS][XF_DIMS]) {
	int i,j;
	for (i = 0; i < XF_DIMS; i++)
		for (j = 0; j < XF_DIMS; j++)
			vals[i][j] = (i == j) ? 1 : 0;
}

#if XF_DIMS == 5
#define IDENT_MAT {{1,0,0,0,0},{0,1,0,0,0},{0,0,1,0,0},{0,0,0,1,0},{0,0,0,0,1}}
#elif XF_DIMS == 4
#define IDENT_MAT {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}
#endif

#define ROT_MAT_FN(id,a,b)					\
	void mat_rot_##id(float rad) {				\
		float mat[XF_DIMS][XF_DIMS] = IDENT_MAT;	\
		mat[a][a] = cos(rad);				\
		mat[a][b] = sin(rad);				\
		mat[b][a] = -sin(rad);				\
		mat[b][b] = cos(rad);				\
		matrix_mul(mat);				\
	}

ROT_MAT_FN(WX,0,1)
ROT_MAT_FN(XY,1,2)
ROT_MAT_FN(YZ,2,3)
ROT_MAT_FN(ZW,3,0)
ROT_MAT_FN(WY,0,2)
ROT_MAT_FN(XZ,1,3)

void transformXYZW(float *pts) {
	float pts0[XF_DIMS];
	int i,j,k;
	for (i = 0; i < XF_DIMS; i++)
		pts0[i] = pts[i];
	for (j = 0; j < XF_DIMS; j++) {
		float f = 0;
		for(i = 0; i < XF_DIMS; i++)
			f += pts0[i] * p_transform[i][j];
		pts[j] = f;
	}
}

